#ifndef __TDP2_MATH_HPP__
#define __TDP2_MATH_HPP__

#include "tdp2.hpp"
#include <math.h>

namespace TDP
{
	namespace Math
	{
		static const double Epsilon_k = 0.0001;
		static const double Pi_k = 3.1415926535897932384626433832795;

		namespace ArrayMath
		{
			template<class _Tfloat>
			void VAdd(const _Tfloat *a, const _Tfloat *b, _Tfloat *result, LargeInt count);

			template<class _Tfloat>
			void VSub(const _Tfloat *a, const _Tfloat *b, _Tfloat *result, LargeInt count);

			template<class _Tfloat>
			void VMul(const _Tfloat *a, const _Tfloat *b, _Tfloat *result, LargeInt count);
		};

		
		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		class Mat;

		template<class _Tfloat, LargeInt _Tdim>
		class Vec
		{
		private:
			_Tfloat _v[_Tdim];

			void Init(const _Tfloat *f);
			static Vec<_Tfloat,3> CrossProduct(const Vec<_Tfloat,3> &v1, const Vec<_Tfloat,3> &v2);

		public:
			Vec();

			explicit Vec(_Tfloat x);
			explicit Vec(const _Tfloat *v);
			Vec(_Tfloat x, _Tfloat y);
			Vec(_Tfloat x, _Tfloat y, _Tfloat z);
			Vec(_Tfloat x, _Tfloat y, _Tfloat z, _Tfloat w);

			operator const _Tfloat *() const;
			operator _Tfloat *();
			Vec<_Tfloat,_Tdim> operator - () const;
			Vec<_Tfloat,_Tdim> operator * (_Tfloat rs) const;
			Vec<_Tfloat,_Tdim> operator / (_Tfloat rs) const;
			Vec<_Tfloat,_Tdim> operator + (_Tfloat rs) const;
			Vec<_Tfloat,_Tdim> operator - (_Tfloat rs) const;
			Vec<_Tfloat,_Tdim> operator * (const Vec<_Tfloat,_Tdim> &rs) const;
			Vec<_Tfloat,_Tdim> operator / (const Vec<_Tfloat,_Tdim> &rs) const;
			Vec<_Tfloat,_Tdim> operator + (const Vec<_Tfloat,_Tdim> &rs) const;
			Vec<_Tfloat,_Tdim> operator - (const Vec<_Tfloat,_Tdim> &rs) const;

			template<LargeInt _Trows>
			Vec<_Tfloat, _Trows> operator % (const Mat<_Tfloat, _Trows, _Tdim> &rs) const;

			Vec<_Tfloat,_Tdim> Minimum(const Vec<_Tfloat,_Tdim> &rs) const;
			Vec<_Tfloat,_Tdim> Maximum(const Vec<_Tfloat,_Tdim> &rs) const;
			_Tfloat DotProduct(const Vec<_Tfloat,_Tdim> &v) const;
			Vec<_Tfloat,3> Cross(const Vec<_Tfloat,_Tdim> &v) const;
			Vec<_Tfloat,_Tdim> Lerp(const Vec<_Tfloat,_Tdim> &v, _Tfloat t) const;
			_Tfloat LengthSquared() const;
			_Tfloat Length() const;
			Vec<_Tfloat,_Tdim> Normalize() const;
			Vec<_Tfloat,_Tdim> Normalize2() const;
			Vec<_Tfloat,_Tdim> FlatAgainst(const Vec<_Tfloat,_Tdim> &rs) const;
			bool operator == (const Vec<_Tfloat,_Tdim> &rs) const;
			bool operator != (const Vec<_Tfloat,_Tdim> &rs) const;
			
			Vec<_Tfloat,_Tdim> &operator *= (const Vec<_Tfloat,_Tdim> &rs);
			Vec<_Tfloat,_Tdim> &operator /= (const Vec<_Tfloat,_Tdim> &rs);
			Vec<_Tfloat,_Tdim> &operator += (const Vec<_Tfloat,_Tdim> &rs);
			Vec<_Tfloat,_Tdim> &operator -= (const Vec<_Tfloat,_Tdim> &rs);

			template<LargeInt _Tnewdim>
			Vec<_Tfloat,_Tnewdim> Reduce() const;

			template<LargeInt _Tnewdim>
			Vec<_Tfloat,_Tnewdim> Expand(_Tfloat filler = 0.0) const;

			Vec<_Tfloat,_Tdim+1> Join(_Tfloat rs) const;

			template<LargeInt _Trsdim>
			Vec<_Tfloat,_Tdim+_Trsdim> Join(const Vec<_Tfloat,_Trsdim> &rs) const;
		};

		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		class Mat
		{
		private:
			Vec<_Tfloat, _Tcols> _m[_Trows];

			static Mat<_Tfloat, 3, 3> Inverse(const Mat<_Tfloat, 3, 3> &self);
			static _Tfloat Determinant(const Mat<_Tfloat, 3, 3> &self);

		public:
			Mat();
			explicit Mat(_Tfloat x);
			explicit Mat(const _Tfloat *v);

			const Vec<_Tfloat, _Tcols> &operator [] (LargeInt idx) const;
			Vec<_Tfloat, _Tcols> &operator [] (LargeInt idx);

			Mat<_Tfloat, _Trows, _Tcols> operator % (const Mat<_Tfloat, _Tcols, _Trows> &rs) const;
			Vec<_Tfloat, _Trows> operator % (const Vec<_Tfloat, _Tcols> &rs) const;
			Mat<_Tfloat, _Tcols, _Trows> Transpose() const;
			Mat<_Tfloat, _Tcols, _Trows> Inverse() const;
			_Tfloat Determinant() const;

			void MAcc(_Tfloat mult, const Mat<_Tfloat, _Trows, _Tcols> &rs);
		};

		template<class _Tfloat, LargeInt _Tdim>
		struct Line
		{
			Vec<_Tfloat,_Tdim> points[2];

			Line();
			Line(const Vec<_Tfloat,_Tdim> &p1, const Vec<_Tfloat,_Tdim> &p2);
			Vec<_Tfloat,_Tdim> Trajectory() const;
		};

		template<class _Tfloat,LargeInt _Tdim>
		struct Plane
		{
			Vec<_Tfloat,_Tdim> normal;
			_Tfloat dist;

			enum Side
			{
				Back,
				On,
				Front,
			};

			Plane();
			Plane(_Tfloat d, const Vec<_Tfloat,_Tdim> &n);
			Plane(const Vec<_Tfloat,_Tdim> &p1, const Vec<_Tfloat,_Tdim> &p2, const Vec<_Tfloat,_Tdim> &p3);
			Plane<_Tfloat,_Tdim> Push(_Tfloat d) const;
			Plane<_Tfloat,_Tdim> Invert() const;
			int PointSide(const Vec<_Tfloat,_Tdim> &p, _Tfloat epsilon) const;
			int PointSide(const Vec<_Tfloat,_Tdim> &p) const;
			bool IntersectWithLineSegment(const Line<_Tfloat,_Tdim> &line, Vec<_Tfloat,_Tdim> &impact) const;
			Vec<_Tfloat,_Tdim> IntersectWithLine(const Line<_Tfloat,_Tdim> &line) const;
		};

		template<class _Tfloat,LargeInt _Tdim>
		struct Triangle
		{
			Vec<_Tfloat,_Tdim> points[3];

			Triangle();
			Triangle(const Vec<_Tfloat,_Tdim> &p1, const Vec<_Tfloat,_Tdim> &p2, const Vec<_Tfloat,_Tdim> &p3);

			template<LargeInt _Tnewdim>
			Triangle<_Tfloat,_Tnewdim> Expand() const;

			Math::Line<_Tfloat,_Tdim> Edge(unsigned int index) const;

			_Tfloat Area() const;

			Math::Plane<_Tfloat,_Tdim> Plane() const;

			Triangle<_Tfloat,_Tdim> Invert() const;

			Math::Plane<_Tfloat,3> ClipPlane(unsigned int edgeNum) const;
		};

		template<class _Tfloat,LargeInt _Tdim>
		struct Bounds
		{
			Vec<_Tfloat,_Tdim> mins;
			Vec<_Tfloat,_Tdim> maxs;
		};

		template<class _Tfloat,LargeInt _Tdim>
		struct Sphere
		{
			Vec<_Tfloat,_Tdim> center;
			_Tfloat radius;

			Sphere();

			Sphere(const Vec<_Tfloat,_Tdim> &c, _Tfloat r);

			int IntersectWithLine(const Line<_Tfloat,_Tdim> &line, Vec<_Tfloat,_Tdim> &p1, Vec<_Tfloat,_Tdim> &p2) const;

			int IntersectWithLine(const Line<_Tfloat,_Tdim> &line) const;

			bool ContainsPoint(const Vec<_Tfloat,_Tdim> &p) const;
		};
		
		template<class _Tfloat>
		class Cylinder
		{
			Line<_Tfloat,3> _line;
			_Tfloat _radius;

			Plane<_Tfloat,3> _capPlanes[2];

			void ComputeCaps();

		public:
			Cylinder();
			Cylinder(const Line<_Tfloat,3> &line, _Tfloat radius);
			Cylinder(const Vec<_Tfloat,3> &p1, const Vec<_Tfloat,3> &p2, _Tfloat radius);
			bool ContainsPoint(const Vec<_Tfloat,3> &p);
		};

		// Triangulators compute triangulations, which are multipliers that can be applied to the triangle points
		// to reproduce the original, or mapped to other triangles
		// They are objects so calculations can be reused
		template<class _Tfloat>
		class Triangulator
		{
		private:
			Triangle<_Tfloat,3> _tri;
			Vec<_Tfloat,3> _abPerpendicular;
			Vec<_Tfloat,3> _bcNormalized;
			Vec<_Tfloat,3> _ab;
			Vec<_Tfloat,3> _bc;

			_Tfloat _abLengthSquared;
			_Tfloat _bcLengthSquared;

			_Tfloat _point1dotABP;
			Vec<_Tfloat,3> _coefMultiplier;

		public:
			explicit Triangulator(const Triangle<_Tfloat,3> &tri);
			Vec<_Tfloat,3> TriangulationForPoint(const Vec<_Tfloat,3> pos) const;
			Vec<_Tfloat,3> PointAtTriangulation(const Vec<_Tfloat,3> &t) const;
		};
		
		typedef Vec<Float32,4> FVec4;
		typedef Vec<Float32,3> FVec3;
		typedef Vec<Float32,2> FVec2;
		typedef Mat<Float32,3,3> FMat3;
		typedef Mat<Float32,4,4> FMat4;
		typedef Plane<Float32,3> FPlane3;
		typedef Line<Float32,3> FLine3;
		typedef Bounds<Float32,3> FBounds3;
		typedef Triangle<Float32,3> FTriangle3;
		typedef Triangle<Float32,2> FTriangle2;
		typedef Sphere<Float32,3> FSphere3;
		typedef Cylinder<Float32> FCylinder;
		typedef Triangulator<Float32> FTriangulator;
	};
};


// ====================================================================================

namespace TDP
{
	namespace Math
	{
		namespace ArrayMath
		{
			template<class _Tfloat>
			void VAdd(const _Tfloat *a, const _Tfloat *b, _Tfloat *result, LargeInt count);

			template<class _Tfloat>
			void VSub(const _Tfloat *a, const _Tfloat *b, _Tfloat *result, LargeInt count);

			template<class _Tfloat>
			void VMul(const _Tfloat *a, const _Tfloat *b, _Tfloat *result, LargeInt count);
		};

		template<class _Tfloat, LargeInt _Tdim>
		void Vec<_Tfloat,_Tdim>::Init(const _Tfloat *f)
		{
			for(LargeInt i=0;i<_Tdim;i++)
				_v[i] = f[i];
		}

		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,3> Vec<_Tfloat,_Tdim>::CrossProduct(const Vec<_Tfloat,3> &v1, const Vec<_Tfloat,3> &v2)
		{
			return Vec<_Tfloat,3>(
				v1[1]*v2[2] - v1[2]*v2[1],
				v1[2]*v2[0] - v1[0]*v2[2],
				v1[0]*v2[1] - v1[1]*v2[0]);
		}

		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim>::Vec() { }
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim>::Vec(_Tfloat x)
		{
			for(LargeInt i=0;i<_Tdim;i++)
				_v[i] = x;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim>::Vec(const _Tfloat *v)
		{
			Init(v);
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim>::Vec(_Tfloat x, _Tfloat y)
		{
			_v[0] = x;
			_v[1] = y;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim>::Vec(_Tfloat x, _Tfloat y, _Tfloat z)
		{
			_v[0] = x;
			_v[1] = y;
			_v[2] = z;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim>::Vec(_Tfloat x, _Tfloat y, _Tfloat z, _Tfloat w)
		{
			_v[0] = x;
			_v[1] = y;
			_v[2] = z;
			_v[3] = w;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim>::operator const _Tfloat *() const
		{
			return _v;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim>::operator _Tfloat *()
		{
			return _v;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::operator - () const
		{
			Vec<_Tfloat,_Tdim> result;
			for(int i=0;i<_Tdim;i++)
				result[i] = -_v[i];
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::operator * (_Tfloat rs) const
		{
			Vec<_Tfloat,_Tdim> result;
			for(LargeInt i=0;i<_Tdim;i++)
				result[i] = _v[i] * rs;
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::operator / (_Tfloat rs) const
		{
			Vec<_Tfloat,_Tdim> result;
			for(LargeInt i=0;i<_Tdim;i++)
				result[i] = _v[i] / rs;
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::operator + (_Tfloat rs) const
		{
			Vec<_Tfloat,_Tdim> result;
			for(LargeInt i=0;i<_Tdim;i++)
				result[i] = _v[i] + rs;
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::operator - (_Tfloat rs) const
		{
			Vec<_Tfloat,_Tdim> result;
			for(LargeInt i=0;i<_Tdim;i++)
				result[i] = _v[i] - rs;
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::operator * (const Vec<_Tfloat,_Tdim> &rs) const
		{
			Vec<_Tfloat,_Tdim> result;
			for(LargeInt i=0;i<_Tdim;i++)
				result[i] = _v[i] * rs[i];
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::operator / (const Vec<_Tfloat,_Tdim> &rs) const
		{
			Vec<_Tfloat,_Tdim> result;
			for(LargeInt i=0;i<_Tdim;i++)
				result[i] = _v[i] / rs[i];
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::operator + (const Vec<_Tfloat,_Tdim> &rs) const
		{
			Vec<_Tfloat,_Tdim> result;
			for(LargeInt i=0;i<_Tdim;i++)
				result[i] = _v[i] + rs[i];
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::operator - (const Vec<_Tfloat,_Tdim> &rs) const
		{
			Vec<_Tfloat,_Tdim> result;
			for(LargeInt i=0;i<_Tdim;i++)
				result[i] = _v[i] - rs[i];
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tcols>
		template<LargeInt _Trows>
		inline Vec<_Tfloat, _Trows> Vec<_Tfloat, _Tcols>::operator % (const Mat<_Tfloat, _Trows, _Tcols> &rs) const
		{
			_Tfloat components[_Tcols];
			for(LargeInt col=0;col<_Trows;col++)
			{
				components[col] = rs[0][col] * _v[0];
				for(LargeInt row=1;row<_Trows;row++)
					components[col] += rs[row][col] * _v[row];
			}
			return Vec<_Tfloat, _Tcols>(components);
		}

		template<class _Tfloat, LargeInt _Tdim>
		Vec<_Tfloat,_Tdim> &Vec<_Tfloat,_Tdim>::operator *= (const Vec<_Tfloat,_Tdim> &rs)
		{
			for(LargeInt i=0;i<_Tdim;i++)
				_v[i] = _v[i] * rs._v[i];
			return *this;
		}

		
		template<class _Tfloat, LargeInt _Tdim>
		Vec<_Tfloat,_Tdim> &Vec<_Tfloat,_Tdim>::operator /= (const Vec<_Tfloat,_Tdim> &rs)
		{
			for(LargeInt i=0;i<_Tdim;i++)
				_v[i] = _v[i] / rs._v[i];
			return *this;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		Vec<_Tfloat,_Tdim> &Vec<_Tfloat,_Tdim>::operator += (const Vec<_Tfloat,_Tdim> &rs)
		{
			for(LargeInt i=0;i<_Tdim;i++)
				_v[i] = _v[i] + rs._v[i];
			return *this;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		Vec<_Tfloat,_Tdim> &Vec<_Tfloat,_Tdim>::operator -= (const Vec<_Tfloat,_Tdim> &rs)
		{
			for(LargeInt i=0;i<_Tdim;i++)
				_v[i] = _v[i] - rs._v[i];
			return *this;
		}

		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::Minimum(const Vec<_Tfloat,_Tdim> &rs) const
		{
			Vec<_Tfloat,_Tdim> result;
			for(LargeInt i=0;i<_Tdim;i++)
			{
				_Tfloat rsi = rs[i];
				_Tfloat vi = _v[i];
				if(rsi < vi)
					result[i] = rsi;
				else
					result[i] = vi;
			}
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::Maximum(const Vec<_Tfloat,_Tdim> &rs) const
		{
			Vec<_Tfloat,_Tdim> result;
			for(LargeInt i=0;i<_Tdim;i++)
			{
				_Tfloat rsi = rs[i];
				_Tfloat vi = _v[i];
				if(rsi > vi)
					result[i] = rsi;
				else
					result[i] = vi;
			}
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline _Tfloat Vec<_Tfloat,_Tdim>::DotProduct(const Vec<_Tfloat,_Tdim> &v) const
		{
			_Tfloat result = _Tfloat(0);
			for(LargeInt i=0;i<_Tdim;i++)
				result += _v[i] * + v[i];
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,3> Vec<_Tfloat,_Tdim>::Cross(const Vec<_Tfloat,_Tdim> &v) const
		{
			return CrossProduct(*this, v);
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::Lerp(const Vec<_Tfloat,_Tdim> &v, _Tfloat t) const
		{
			_Tfloat nt = _Tfloat(1.0) - t;
			Vec<_Tfloat,_Tdim> result;
			for(LargeInt i=0;i<_Tdim;i++)
				result[i] = _v[i]*nt+v[i]*t;
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline _Tfloat Vec<_Tfloat,_Tdim>::LengthSquared() const
		{
			_Tfloat result = _Tfloat(0);
			for(LargeInt i=0;i<_Tdim;i++)
				result += _v[i]*_v[i];
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline _Tfloat Vec<_Tfloat,_Tdim>::Length() const
		{
			return _Tfloat(sqrt(this->LengthSquared()));
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::Normalize() const
		{
			Vec<_Tfloat,_Tdim> result;
			_Tfloat len = this->Length();
			if(len < 0.0001)
			{
				result[0] = 1.0;
				for(LargeInt i=1;i<_Tdim;i++)
					result[i] = 0.0;
				return result;
			}


			for(LargeInt i=0;i<_Tdim;i++)
				result[i] = _v[i]/len;
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::FlatAgainst(const Vec<_Tfloat,_Tdim> &rs) const
		{
			return (*this) - (*this) * this->DotProduct(rs);
		}

		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Vec<_Tfloat,_Tdim>::Normalize2() const
		{
			Vec<_Tfloat,_Tdim> result;
			_Tfloat len = this->Length();
			if(len < 0.0001)
			{
				for(LargeInt i=0;i<_Tdim;i++)
					result[i] = 0.0;
				return result;
			}


			for(LargeInt i=0;i<_Tdim;i++)
				result[i] = _v[i]/len;
			return result;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline bool Vec<_Tfloat,_Tdim>::operator == (const Vec<_Tfloat,_Tdim> &rs) const
		{
			for(LargeInt i=0;i<_Tdim;i++)
				if(rs[i] != _v[i])
					return false;
			return true;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline bool Vec<_Tfloat,_Tdim>::operator != (const Vec<_Tfloat,_Tdim> &rs) const
		{
			for(LargeInt i=0;i<_Tdim;i++)
				if(rs[i] != _v[i])
					return true;
			return false;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		template<LargeInt _Tnewdim>
		inline Vec<_Tfloat,_Tnewdim> Vec<_Tfloat,_Tdim>::Reduce() const
		{
			return Vec<_Tfloat,_Tnewdim>(_v);
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		template<LargeInt _Tnewdim>
		inline Vec<_Tfloat,_Tnewdim> Vec<_Tfloat,_Tdim>::Expand(_Tfloat filler = 0.0) const
		{
			Vec<_Tfloat,_Tnewdim> r(filler);
			for(LargeInt i=0;i<_Tdim;i++)
				r[i] = _v[i];
			return r;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		Vec<_Tfloat,_Tdim+1> Vec<_Tfloat,_Tdim>::Join(_Tfloat rs) const
		{
			Vec<_Tfloat,_Tdim+1> r;
			for(LargeInt i=0;i<_Tdim;i++)
				r[i] = _v[i];
			r[_Tdim] = rs;
			return r;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		template<LargeInt _Trsdim>
		Vec<_Tfloat,_Tdim+_Trsdim> Vec<_Tfloat,_Tdim>::Join(const Vec<_Tfloat,_Trsdim> &rs) const
		{
			Vec<_Tfloat,_Tdim+_Trsdim> r;
			for(LargeInt i=0;i<_Tdim;i++)
				r[i] = _v[i];
			for(LargeInt i=0;i<_Trsdim;i++)
				r[_Tdim+i] = rs[i];
			return r;
		}

		// =============================================================================
		// Matrix
		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		inline Mat<_Tfloat, _Trows, _Tcols>::Mat()
		{
		}

		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		inline Mat<_Tfloat, _Trows, _Tcols>::Mat(_Tfloat x)
		{
			for(LargeInt row=0;row<_Trows;row++)
				_m[row] = Vec<_Tfloat, _Tcols>(x);
		}

		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		inline Mat<_Tfloat, _Trows, _Tcols>::Mat(const _Tfloat *v)
		{
			for(LargeInt row=0;row<_Trows;row++)
			{
				_m[row] = Vec<_Tfloat, _Tcols>(v);
				v += _Tcols;
			}
		}

		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		inline const Vec<_Tfloat, _Tcols> &Mat<_Tfloat, _Trows, _Tcols>::operator [] (LargeInt idx) const
		{
			return _m[idx];
		}

		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		inline Vec<_Tfloat, _Tcols> &Mat<_Tfloat, _Trows, _Tcols>::operator [] (LargeInt idx)
		{
			return _m[idx];
		}

		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		inline Vec<_Tfloat, _Trows> Mat<_Tfloat, _Trows, _Tcols>::operator % (const Vec<_Tfloat, _Tcols> &rs) const
		{
			_Tfloat components[_Trows];
			for(LargeInt row=0;row<_Trows;row++)
			{
				components[row] = _m[row][0] * rs[0];
				for(LargeInt col=1;col<_Tcols;col++)
					components[row] += _m[row][col] * rs[col];
			}
			return Vec<_Tfloat, _Trows>(components);
		}

		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		inline Mat<_Tfloat, _Tcols, _Trows> Mat<_Tfloat, _Trows, _Tcols>::Transpose() const
		{
			Mat<_Tfloat, _Tcols, _Trows> result;
			for(LargeInt row=0;row<_Trows;row++)
				for(LargeInt col=0;col<_Tcols;col++)
					result._m[col][row] = _m[row][col];
			return result;
		}

		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		inline Mat<_Tfloat, 3, 3> Mat<_Tfloat, _Trows, _Tcols>::Inverse(const Mat<_Tfloat, 3, 3> &self)
		{
			Mat<_Tfloat, 3, 3> result;
			_Tfloat a = self[0][0];
			_Tfloat b = self[0][1];
			_Tfloat c = self[0][2];
			_Tfloat d = self[1][0];
			_Tfloat e = self[1][1];
			_Tfloat f = self[1][2];
			_Tfloat g = self[2][0];
			_Tfloat h = self[2][1];
			_Tfloat k = self[2][2];
			_Tfloat det = a*(e*k - f*h) - b*(d*k - f*g) + c*(d*h - e*g);

			if(det == _Tfloat(0))
				return Mat<_Tfloat, 3, 3>(_Tfloat(0));

			_Tfloat coefs[9];
			coefs[0] = (e*k - f*h) / det;
			coefs[1] = (f*g - d*k) / det;
			coefs[2] = (d*h - e*g) / det;
			coefs[3] = (c*h - b*k) / det;
			coefs[4] = (a*k - c*g) / det;
			coefs[5] = (b*g - a*h) / det;
			coefs[6] = (b*f - c*e) / det;
			coefs[7] = (c*d - a*f) / det;
			coefs[8] = (a*e - b*d) / det;
			return Mat<_Tfloat, 3, 3>(coefs);
		}
		
		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		inline Mat<_Tfloat, _Tcols, _Trows> Mat<_Tfloat, _Trows, _Tcols>::Inverse() const
		{
			return Inverse(*this);
		}

		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		inline _Tfloat Mat<_Tfloat, _Trows, _Tcols>::Determinant(const Mat<_Tfloat, 3, 3> &self)
		{
			return self[0][0] * (self[1][1] * self[2][2] - self[1][2] * self[2][1]) -
				self[0][1] * (self[2][2] * self[1][0] - self[1][2] * self[0][2]) +
				self[0][2] * (self[1][0] * self[1][2] - self[1][1] * self[0][2]);
		}
		

		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		inline _Tfloat Mat<_Tfloat, _Trows, _Tcols>::Determinant() const
		{
			return Determinant(*this);
		}

		template<class _Tfloat, LargeInt _Trows, LargeInt _Tcols>
		inline void Mat<_Tfloat, _Trows, _Tcols>::MAcc(_Tfloat mult, const Mat<_Tfloat, _Trows, _Tcols> &rs)
		{
			for(LargeInt row=0;row<_Trows;row++)
				for(LargeInt col=0;col<_Tcols;col++)
					_m[row][col] += mult * rs._m[row][col];
		}

		// =============================================================================
		// Line

		template<class _Tfloat, LargeInt _Tdim>
		inline Line<_Tfloat,_Tdim>::Line()
		{
		}

		template<class _Tfloat, LargeInt _Tdim>
		inline Line<_Tfloat,_Tdim>::Line(const Vec<_Tfloat,_Tdim> &p1, const Vec<_Tfloat,_Tdim> &p2)
		{
			points[0] = p1;
			points[1] = p2;
		}

		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Line<_Tfloat,_Tdim>::Trajectory() const
		{
			return points[1] - points[0];
		}
		
		// =============================================================================
		// Plane
		template<class _Tfloat, LargeInt _Tdim>
		inline Plane<_Tfloat,_Tdim>::Plane()
		{
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Plane<_Tfloat,_Tdim>::Plane(_Tfloat d, const Vec<_Tfloat,_Tdim> &n)
		{
			dist = d;
			normal = n;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Plane<_Tfloat,_Tdim>::Plane(const Vec<_Tfloat,_Tdim> &p1, const Vec<_Tfloat,_Tdim> &p2, const Vec<_Tfloat,_Tdim> &p3)
		{
			normal = (p1 - p2).Cross(p3 - p2).Normalize2();
			dist = normal.DotProduct(p1);
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Plane<_Tfloat,_Tdim> Plane<_Tfloat,_Tdim>::Push(_Tfloat d) const
		{
			return Plane<_Tfloat,_Tdim>(dist + d, normal);
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Plane<_Tfloat,_Tdim> Plane<_Tfloat,_Tdim>::Invert() const
		{
			return Plane<_Tfloat,_Tdim>(-dist, -normal);
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline int Plane<_Tfloat,_Tdim>::PointSide(const Vec<_Tfloat,_Tdim> &p, _Tfloat epsilon) const
		{
			_Tfloat d = p.DotProduct(normal) - dist;

			if(d > epsilon)
				return Front;
			if(d < -epsilon)
				return Back;
			return On;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline int Plane<_Tfloat,_Tdim>::PointSide(const Vec<_Tfloat,_Tdim> &p) const
		{
			return PointSide(p, _Tfloat(Math::Epsilon_k));
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		bool Plane<_Tfloat,_Tdim>::IntersectWithLineSegment(const Line<_Tfloat,_Tdim> &line, Vec<_Tfloat,_Tdim> &impact) const
		{
			_Tfloat dist1, dist2;
			
			dist1 = line.point1.DotProduct(normal);
			dist2 = line.point2.DotProduct(_normal);

			if(dist1 == dist2)
			{
				impact = (line.point1 + line.point2) * _Tfloat(0.5);
				return false;
			}

			_Tfloat frac = (dist - dist1) / (dist2 - dist1);
			impact = line.point1 + (line.point2 - line.point1) * frac;

			return ( (dist1 >= _distance) != (dist2 >= _distance) );
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Vec<_Tfloat,_Tdim> Plane<_Tfloat,_Tdim>::IntersectWithLine(const Line<_Tfloat,_Tdim> &line) const
		{
			Vec<_Tfloat,_Tdim> scrap;
			IntersectWithLineSegment(line, scrap);
			return scrap;
		}

		// =============================================================================
		// Triangle

		template<class _Tfloat, LargeInt _Tdim>
		inline Triangle<_Tfloat,_Tdim>::Triangle()
		{
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Triangle<_Tfloat,_Tdim>::Triangle(const Vec<_Tfloat,_Tdim> &p1, const Vec<_Tfloat,_Tdim> &p2, const Vec<_Tfloat,_Tdim> &p3)
		{
			points[0] = p1;
			points[1] = p2;
			points[2] = p3;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		template<LargeInt _Tnewdim>
		inline Triangle<_Tfloat,_Tnewdim> Triangle<_Tfloat,_Tdim>::Expand() const
		{
			return Triangle<_Tfloat,_Tnewdim>(points[0].Expand<_Tnewdim>(), points[1].Expand<_Tnewdim>(), points[2].Expand<_Tnewdim>());
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Math::Line<_Tfloat,_Tdim> Triangle<_Tfloat,_Tdim>::Edge(unsigned int index) const
		{
			return Math::Line<_Tfloat,_Tdim>(points[index], points[(index+1)%3]);
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline _Tfloat Triangle<_Tfloat,_Tdim>::Area() const
		{
			return (points[0] - points[1]).Cross(points[2] - points[1]).Length() * _Tfloat(0.5);
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Math::Plane<_Tfloat,_Tdim> Triangle<_Tfloat,_Tdim>::Plane() const
		{
			return Math::Plane<_Tfloat,_Tdim>(points[0], points[1], points[2]);
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Triangle<_Tfloat,_Tdim> Triangle<_Tfloat,_Tdim>::Invert() const
		{
			return Triangle<_vecT>(points[1], points[0], points[2]);
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Math::Plane<_Tfloat,3> Triangle<_Tfloat,_Tdim>::ClipPlane(unsigned int edgeNum) const
		{
			Vec<_Tfloat,3> expandedPoints[3];
			for(int i=0;i<3;i++)
				expandedPoints[i] = points[i].Expand<3>(0.0f);
			Vec<_Tfloat,3> triNormal = (expandedPoints[0]-expandedPoints[1]).Cross(expandedPoints[2]-expandedPoints[1]);

			Vec<_Tfloat,3> edge = expandedPoints[((edgeNum+1) % 3)] - expandedPoints[edgeNum];
			Vec<_Tfloat,3> planeNormal = edge.Cross(triNormal).Normalize2();
			return Math::Plane<_Tfloat,3>(planeNormal.DotProduct(expandedPoints[edgeNum]), planeNormal);
		}
		
		// =============================================================================
		// Sphere
		template<class _Tfloat, LargeInt _Tdim>
		inline Sphere<_Tfloat,_Tdim>::Sphere()
		{
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline Sphere<_Tfloat,_Tdim>::Sphere(const Vec<_Tfloat,_Tdim> &c, _Tfloat r)
		{
			center = c;
			radius = r;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		int Sphere<_Tfloat,_Tdim>::IntersectWithLine(const Line<_Tfloat,_Tdim> &line, Vec<_Tfloat,_Tdim> &p1, Vec<_Tfloat,_Tdim> &p2) const
		{
			Vec<_Tfloat,_Tdim> offsetCenter = _center - line.Point(0);
			Vec<_Tfloat,_Tdim> traj = line.Trajectory().Normalize2();

			_Tfloat centerDotTraj = offsetCenter.DotProduct(traj);

			_Tfloat qFactor = centerDotTraj*centerDotTraj - offsetCenter.DotProduct(offsetCenter) + radius*radius;

			if(qFactor < -Epsilon_k) return 0;	// Outside completely
			if(qFactor <= Epsilon_k)
			{
				// On the edge
				_Tfloat d = centerDotTraj;
				p1 = traj * centerDotTraj + line.Point(0);
				return 1;
			}

			_Tfloat sqrtQFactor = _Tfloat(sqrtf(qFactor));
			p1 = traj * (centerDotTraj + sqrtQFactor) + line.Point(0);
			p2 = traj * (centerDotTraj - sqrtQFactor) + line.Point(0);

			return 2;
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline int Sphere<_Tfloat,_Tdim>::IntersectWithLine(const Line<_Tfloat,_Tdim> &line) const
		{
			Vec<_Tfloat,_Tdim> scrap1, scrap2;
			return IntersectWithLine(line, scrap1, scrap2);
		}
		
		template<class _Tfloat, LargeInt _Tdim>
		inline bool Sphere<_Tfloat,_Tdim>::ContainsPoint(const Vec<_Tfloat,_Tdim> &p) const
		{
			return (p - center).LengthSquared() <= radius*radius;
		}
		
		// =============================================================================
		// Cylinder
		template<class _Tfloat>
		inline void Cylinder<_Tfloat>::ComputeCaps()
		{
			Vec<_Tfloat,3> normal = _line.Trajectory().Normalize2();
			_capPlanes[0] = Plane<_Tfloat,3>(-normal.DotProduct(_line.points[0]), -normal);
			_capPlanes[1] = Plane<_Tfloat,3>(normal.DotProduct(_line.points[1]), normal);
		}
		
		template<class _Tfloat>
		inline Cylinder<_Tfloat>::Cylinder()
		{
		}
		
		template<class _Tfloat>
		inline Cylinder<_Tfloat>::Cylinder(const Line<_Tfloat,3> &line, _Tfloat radius)
		{
			_line = line;
			_radius = radius;
			ComputeCaps();
		}
		
		template<class _Tfloat>
		inline Cylinder<_Tfloat>::Cylinder(const Vec<_Tfloat,3> &p1, const Vec<_Tfloat,3> &p2, _Tfloat radius)
		{
			_line = Line<_Tfloat,3>(p1, p2);
			_radius = radius;
			ComputeCaps();
		}
		
		template<class _Tfloat>
		inline bool Cylinder<_Tfloat>::ContainsPoint(const Vec<_Tfloat,3> &p)
		{
			// See if this is past either cap
			if(_capPlanes[0].PointSide(p) == Plane<_Tfloat,3>::Front)
				return false;
			if(_capPlanes[1].PointSide(p) == Plane<_Tfloat,3>::Front)
				return false;

			// Translate the container point to one of the caps
			_Tfloat distOffP1 = _capPlanes[0].normal.DotProduct(p) - _capPlanes[0].dist;
			Vec<_Tfloat,3> capPoint = p - _capPlanes[0].normal * distOffP1;

			// See if this is within range of the cap
			return (capPoint - _line.points[0]).LengthSquared() <= _radius*_radius;
		}

		// Triangulators compute triangulations, which are multipliers that can be applied to the triangle points
		// to reproduce the original, or mapped to other triangles
		// They are objects so calculations can be reused
		template<class _Tfloat>
		inline Triangulator<_Tfloat>::Triangulator(const Triangle<_Tfloat,3> &tri)
		{
			_tri = tri;
			_ab = _tri.points[1] - _tri.points[0];
			_bc = _tri.points[2] - _tri.points[1];
			Vec<_Tfloat,3> abNormalized = _ab.Normalize2();
			_bcNormalized = _bc.Normalize2();
			Vec<_Tfloat,3> planeNormal = (_ab.Cross(_bc).Normalize2());

			_abPerpendicular = planeNormal.Cross(abNormalized).Normalize2();

			_abLengthSquared = _ab.LengthSquared();
			_bcLengthSquared = _bc.LengthSquared();

			_point1dotABP = _tri.points[1].DotProduct(_abPerpendicular);
			_coefMultiplier = _bcNormalized / (_abPerpendicular.DotProduct(_bcNormalized));
		}
		
		template<class _Tfloat>
		inline Vec<_Tfloat,3> Triangulator<_Tfloat>::TriangulationForPoint(const Vec<_Tfloat,3> pos) const
		{
			// Compute a location which is on the AB line, translated in the direction of BC
			Vec<_Tfloat,3> abOnPos = pos - _coefMultiplier * (_abPerpendicular.DotProduct(pos) - _point1dotABP);

			_Tfloat abFrac = (abOnPos - _tri.points[0]).DotProduct(_ab) / _abLengthSquared;
			_Tfloat bcFrac = (pos - abOnPos).DotProduct(_bc) / _bcLengthSquared;
			
			return Vec<_Tfloat,3>(_Tfloat(1.0) - abFrac, abFrac - bcFrac, bcFrac);
		}
		
		template<class _Tfloat>
		inline Vec<_Tfloat,3> Triangulator<_Tfloat>::PointAtTriangulation(const Vec<_Tfloat,3> &t) const
		{
			return _tri.points[0]*t[0] + _tri.points[1]*t[1] + _tri.points[2]*t[2];
		}
	};
};

#endif
